//package com.seventh.blekit.fix0629;
//
///**
// * @author chengxin
// * create at 2021/6/29
// * description:
// */
//public class ParserOriginal {
//
//
//
//    /***********************宏定义***********************/
//    private static final float Kp = 40.0f; //比例增益控制加速度计/磁力计的收敛速度
//    private static final float Ki = 0.02f;  //Ki积分增益 决定了陀螺仪偏差的收敛速度
//    private static final float halfT = 0.004f;  //halfT采样周期的一半
//    private static final float =dt 0.008f;
//    //#define dt 0.008f
//
//
//    /***************************************************/
//
//    //四元素变量声明,
//    private volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to auxiliary frame
//    //float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滚角
//
//    //----------------------
//
//    //变量声明：
//    //功能：融合加速度计和磁力计进行姿态调整
//    private static float exInt = 0, eyInt = 0, ezInt = 0;
//    private static float k10 = 0.0f, k11 = 0.0f, k12 = 0.0f, k13 = 0.0f;
//    private static float k20 = 0.0f, k21 = 0.0f, k22 = 0.0f, k23 = 0.0f;
//    private static float k30 = 0.0f, k31 = 0.0f, k32 = 0.0f, k33 = 0.0f;
//    private static float k40 = 0.0f, k41 = 0.0f, k42 = 0.0f, k43 = 0.0f;
//
//    //-------------------------
//    //计算快速平方
//    public static float invSqrt(float x) {
//        float xhalf = 0.5f * x;
//        int i = Float.floatToIntBits(x);
//        i = 0x5f3759df - (i >> 1);
//        x = Float.intBitsToFloat(i);
//        x *= (1.5f - (xhalf * x * x));
//        return x;
//    }
//
//    /**
//     * 其中gx,gy,gx,
//     * ax,ay,az,
//     * mx,my,mz,
//     * 为接收到原始数据转换后的浮点数数据，
//     * <p>
//     * 如接收到的数据：
//     * short gyro_x; //表示接收到的gx轴的原始数据，经过判断正负方向后，
//     * <p>
//     * float gx = (float)(gyro_x/100); //得到结果后传入参数，其他轴类似
//     * <p>
//     * 调用函数后可以得到四元数值，q0,q1,q2,q3
//     */
//    static void mpu_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
//
//        float norm;                                    //用于单位化
//        float hx, hy, hz, bx, bz;        //
//        float vx, vy, vz, wx, wy, wz;
//        float ex, ey, ez;
//        // 辅助变量减少重复操作次数用于加速运算
//        float q0q0 = q0 * q0;
//        float q0q1 = q0 * q1;
//        float q0q2 = q0 * q2;
//        float q0q3 = q0 * q3;
//        float q1q1 = q1 * q1;
//        float q1q2 = q1 * q2;
//        float q1q3 = q1 * q3;
//        float q2q2 = q2 * q2;
//        float q2q3 = q2 * q3;
//        float q3q3 = q3 * q3;
//
//        //对加速度计和磁力计数据进行规范化
//        norm = invSqrt(ax * ax + ay * ay + az * az);
//        ax = ax * norm;
//        ay = ay * norm;
//        az = az * norm;
//        norm = invSqrt(mx * mx + my * my + mz * mz);
//        mx = mx * norm;
//        my = my * norm;
//        mz = mz * norm;
//
//        //计算磁场的参考方向
//        //hx,hy,hz是mx,my,mz在参考坐标系的表示
//        hx = 2 * mx * (0.50 - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
//        hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.50 - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
//        hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.50 - q1q1 - q2q2);
//        //bx,by,bz是地球磁场在参考坐标系的表示
//        bx = sqrt((hx * hx) + (hy * hy));
//        bz = hz;
//
//        //预估重力和磁场的方向(v、w)
//        //vx,vy,vz是重力加速度在物体坐标系的表示
//        vx = 2 * (q1q3 - q0q2);
//        vy = 2 * (q0q1 + q2q3);
//        vz = q0q0 - q1q1 - q2q2 + q3q3;
//        //wx,wy,wz是地磁场在物体坐标系的表示
//        wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
//        wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
//        wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);
//
//        //ex,ey,ez是加速度计与磁力计测量出的方向与实际重力加速度与地磁场方向的误差，误差用叉积来表示，且加速度计与磁力计的权重是一样的
//        ex = (ay * vz - az * vy) + (my * wz - mz * wy);
//        ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
//        ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
//        //积分误差
//        exInt = exInt + ex * Ki * dt;
//        eyInt = eyInt + ey * Ki * dt;
//        ezInt = ezInt + ez * Ki * dt;
//        // printf("exInt=%0.1f eyInt=%0.1f ezInt=%0.1f ",exInt,eyInt,ezInt);
//
//        //PI调节陀螺仪数据
//        gx = gx + Kp * ex + exInt;
//        gy = gy + Kp * ey + eyInt;
//        gz = gz + Kp * ez + ezInt;
//        //// 法解微分方程
//        k10 = 0.5 * (-gx * q1 - gy * q2 - gz * q3);
//        k11 = 0.5 * (gx * q0 + gz * q2 - gy * q3);
//        k12 = 0.5 * (gy * q0 - gz * q1 + gx * q3);
//        k13 = 0.5 * (gz * q0 + gy * q1 - gx * q2);
//
//        k20 = 0.5 * (halfT * (q0 + halfT * k10) + (halfT - gx) * (q1 + halfT * k11) + (halfT - gy) * (q2 + halfT * k12) + (halfT - gz) * (q3 + halfT * k13));
//        k21 = 0.5 * ((halfT + gx) * (q0 + halfT * k10) + halfT * (q1 + halfT * k11) + (halfT + gz) * (q2 + halfT * k12) + (halfT - gy) * (q3 + halfT * k13));
//        k22 = 0.5 * ((halfT + gy) * (q0 + halfT * k10) + (halfT - gz) * (q1 + halfT * k11) + halfT * (q2 + halfT * k12) + (halfT + gx) * (q3 + halfT * k13));
//        k23 = 0.5 * ((halfT + gz) * (q0 + halfT * k10) + (halfT + gy) * (q1 + halfT * k11) + (halfT - gx) * (q2 + halfT * k12) + halfT * (q3 + halfT * k13));
//
//        k30 = 0.5 * (halfT * (q0 + halfT * k20) + (halfT - gx) * (q1 + halfT * k21) + (halfT - gy) * (q2 + halfT * k22) + (halfT - gz) * (q3 + halfT * k23));
//        k31 = 0.5 * ((halfT + gx) * (q0 + halfT * k20) + halfT * (q1 + halfT * k21) + (halfT + gz) * (q2 + halfT * k22) + (halfT - gy) * (q3 + halfT * k23));
//        k32 = 0.5 * ((halfT + gy) * (q0 + halfT * k20) + (halfT - gz) * (q1 + halfT * k21) + halfT * (q2 + halfT * k22) + (halfT + gx) * (q3 + halfT * k23));
//        k33 = 0.5 * ((halfT + gz) * (q0 + halfT * k20) + (halfT + gy) * (q1 + halfT * k21) + (halfT - gx) * (q2 + halfT * k22) + halfT * (q3 + halfT * k23));
//
//        k40 = 0.5 * (dt * (q0 + dt * k30) + (dt - gx) * (q1 + dt * k31) + (dt - gy) * (q2 + dt * k32) + (dt - gz) * (q3 + dt * k33));
//        k41 = 0.5 * ((dt + gx) * (q0 + dt * k30) + dt * (q1 + dt * k31) + (dt + gz) * (q2 + dt * k32) + (dt - gy) * (q3 + dt * k33));
//        k42 = 0.5 * ((dt + gy) * (q0 + dt * k30) + (dt - gz) * (q1 + dt * k31) + dt * (q2 + dt * k32) + (dt + gx) * (q3 + dt * k33));
//        k43 = 0.5 * ((dt + gz) * (q0 + dt * k30) + (dt + gy) * (q1 + dt * k31) + (dt - gx) * (q2 + dt * k32) + dt * (q3 + dt * k33));
//
//        q0 = q0 + dt / 6.0 * (k10 + 2 * k20 + 2 * k30 + k40);
//        q1 = q1 + dt / 6.0 * (k11 + 2 * k21 + 2 * k31 + k41);
//        q2 = q2 + dt / 6.0 * (k12 + 2 * k22 + 2 * k32 + k42);
//        q3 = q3 + dt / 6.0 * (k13 + 2 * k23 + 2 * k33 + k43);
//
//        // 正常化四元数
//        norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
//        q0 = q0 * norm;
//        q1 = q1 * norm;
//        q2 = q2 * norm;
//        q3 = q3 * norm;
//
//         //Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2)* 57.3;	// pitch
//         //Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1)* 57.3;	// roll
//         //Yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
//    }
//}
